#include <stdbool.h>
#include <string.h>
#include "math.h"
#include "config.h"
#include "config_param.h"
#include "watchdog.h"
#include "stmflash.h"
#include "delay.h"

/*FreeRTOS相关头文件*/
#include "FreeRTOS.h"
#include "task.h"
#include "semphr.h"
#include "queue.h"

/********************************************************************************	 
 * 本程序只供学习使用，未经作者许可，不得用于其它任何用途
 * ALIENTEK MiniFly
 * 配置参数驱动代码	
 * 正点原子@ALIENTEK
 * 技术论坛:www.openedv.com
 * 创建日期:2017/5/12
 * 版本：V1.3
 * 版权所有，盗版必究。
 * Copyright(C) 广州市星翼电子科技有限公司 2014-2024
 * All rights reserved
********************************************************************************/


#define VERSION 13 /*13 表示V1.3*/

configParam_t configParam;

static configParam_t configParamDefault =
    {
        .version = VERSION, /*软件版本号*/

        .pidAngle =  /*角度PID*/
        {
                                  .roll =
                {
                    .kp = 8.0,
                    .ki = 0.0,
                    .kd = 0.0,
                },
                                  .pitch =
                {
                    .kp = 8.0,
                    .ki = 0.0,
                    .kd = 0.0,
                },
                                  .yaw =
                {
                    .kp = 20.0,
                    .ki = 0.0,
                    .kd = 1.5,
                },
                                  },
        .pidRate =  /*角速度PID*/
        {
                                  .roll =
                {
                    .kp = 300.0,
                    .ki = 0.0,
                    .kd = 6.5,
                },
                                  .pitch =
                {
                    .kp = 300.0,
                    .ki = 0.0,
                    .kd = 6.5,
                },
                                  .yaw =
                {
                    .kp = 200.0,
                    .ki = 18.5,
                    .kd = 0.0,
                },
                                  },
        .pidPos =  /*位置PID*/
        {
                                  .vx =
                {
                    .kp = 4.5,
                    .ki = 0.0,
                    .kd = 0.0,
                },
                                  .vy =
                {
                    .kp = 4.5,
                    .ki = 0.0,
                    .kd = 0.0,
                },
                                  .vz =
                {
                    .kp = 100.0,
                    .ki = 150.0,
                    .kd = 10.0,
                },

                                  .x =
                {
                    .kp = 4.0,
                    .ki = 0.0,
                    .kd = 0.6,
                },
                                  .y =
                {
                    .kp = 4.0,
                    .ki = 0.0,
                    .kd = 0.6,
                },
                                  .z =
                {
                    .kp = 6.0,
                    .ki = 0.0,
                    .kd = 4.5,
                },
                                  },

        .trimP      = 0.f, /*pitch微调*/
        .trimR      = 0.f, /*roll微调*/
        .thrustBase = 34000, /*定高油门基础值*/
};

static u32  lenth           = 0;
static bool isInit          = false;
static bool isConfigParamOK = false;

static SemaphoreHandle_t xSemaphore = NULL;


static u8 configParamCksum(configParam_t *data)
{
    int    i;
    u8     cksum = 0;
    u8    *c     = (u8 *)data;
    size_t len   = sizeof(configParam_t);

    for (i = 0; i < len; i++)
        cksum += *(c++);
    cksum -= data->cksum;

    return cksum;
}

void configParamInit(void) /*参数配置初始化*/
{
    if (isInit) return;

    lenth = sizeof(configParam);
    lenth = lenth / 4 + (lenth % 4 ? 1 : 0);

    STMFLASH_Read(CONFIG_PARAM_ADDR, (u32 *)&configParam, lenth);

    if (configParam.version == VERSION)                          /*版本正确*/
    {
        if (configParamCksum(&configParam) == configParam.cksum) /*校验正确*/
        {
            printf("Version V%1.1f check [OK]\r\n", configParam.version / 10.0f);
            isConfigParamOK = true;
        }
        else
        {
            printf("Version check [FAIL]\r\n");
            isConfigParamOK = false;
        }
    }
    else /*版本更新*/
    {
        isConfigParamOK = false;
    }

    if (isConfigParamOK == false) /*配置参数错误，写入默认参数*/
    {
        memcpy((u8 *)&configParam, (u8 *)&configParamDefault, sizeof(configParam));
        configParam.cksum = configParamCksum(&configParam);            /*计算校验值*/
        STMFLASH_Write(CONFIG_PARAM_ADDR, (u32 *)&configParam, lenth); /*写入stm32 flash*/
        isConfigParamOK = true;
    }

    xSemaphore = xSemaphoreCreateBinary();

    isInit = true;
}

void configParamTask(void *param)
{
    u8 cksum = 0;

    while (1)
    {
        xSemaphoreTake(xSemaphore, portMAX_DELAY);
        cksum = configParamCksum(&configParam); /*数据校验*/

        if (configParam.cksum != cksum)
        {
            configParam.cksum = cksum;                                     /*数据校验*/
            watchdogInit(500);                                             /*擦除时间比较长，看门狗时间设置大一些*/
            STMFLASH_Write(CONFIG_PARAM_ADDR, (u32 *)&configParam, lenth); /*写入stm32 flash*/
            watchdogInit(WATCHDOG_RESET_MS);                               /*重新设置看门狗*/
        }
    }
}

bool configParamTest(void)
{
    return isInit;
}

void configParamGiveSemaphore(void)
{
    xSemaphoreGive(xSemaphore);
}

void resetConfigParamPID(void)
{
    configParam.pidAngle = configParamDefault.pidAngle;
    configParam.pidRate  = configParamDefault.pidRate;
    configParam.pidPos   = configParamDefault.pidPos;
}

void saveConfigAndNotify(void)
{
    u8 cksum = configParamCksum(&configParam); /*数据校验*/
    if (configParam.cksum != cksum)
    {
        configParam.cksum = cksum;                                     /*数据校验*/
        STMFLASH_Write(CONFIG_PARAM_ADDR, (u32 *)&configParam, lenth); /*写入stm32 flash*/
    }
}
